#!/bin/bash

# ROS2 Floor Parse Package - 统一管理脚本
# 包含所有功能：构建、运行、状态检查、管理等

set -e

# 配置参数
## 会生成文件夹$WORKSPACE_DIR/$PACKAGE_NAME
PACKAGE_NAME="ros2_node_pub_singe_py_node"
WORKSPACE_DIR="/home/ayler/ProjectWorkspace/unitree-robot-projects/"
# foxy最高支持python3.8(确定版本：/bin/python3 --version)
PYTHON_CMD_DIR="/bin/python3"
SOURCE_PY_FILE_DIR="/home/ayler/ProjectWorkspace/unitree-robot-projects/ros2_node_pub_singe_py/src/"
SOURCE_PY_FILE_NAME="floor_publisher.py"
# node 名称(根据py发布节点名称确定)
NODE_NAME="floor_publisher"
LOG_FILE_DIR="/tmp"
#发布节点的py文件路径
SOURCE_PY_FILE="$SOURCE_PY_FILE_DIR$SOURCE_PY_FILE_NAME"

# 源文件路径配置 - 支持通过环境变量覆盖
if [ -n "$SOURCE_PY_FILE" ]; then
    PYTHON_FILE="$SOURCE_PY_FILE"
else
    # 智能查找源文件
    POSSIBLE_PATHS=(
        "$WORKSPACE_DIR/$PACKAGE_NAME/$SOURCE_PY_FILE_NAME"
        "$WORKSPACE_DIR/$PACKAGE_NAME/floor_parse_ros/$SOURCE_PY_FILE_NAME"
        "$WORKSPACE_DIR/$SOURCE_PY_FILE_NAME"
    )
    
    PYTHON_FILE=""
    for path in "${POSSIBLE_PATHS[@]}"; do
        if [ -f "$path" ]; then
            PYTHON_FILE="$path"
            break
        fi
    done
fi

LOG_FILE="$LOG_FILE_DIR/$NODE_NAME.log"

# 验证源文件函数
validate_source_file() {
    if [ -z "$PYTHON_FILE" ] || [ ! -f "$PYTHON_FILE" ]; then
        print_msg $RED "❌ 错误: 找不到源文件!"
        print_msg $YELLOW "当前配置的源文件路径: $PYTHON_FILE"
        print_msg $CYAN "可用解决方案:"
        echo "  1. 设置环境变量: export SOURCE_PY_FILE=/path/to/your/$SOURCE_PY_FILE_NAME"
        echo "  2. 确保文件存在于以下位置之一:"
        for path in "${POSSIBLE_PATHS[@]}"; do
            echo "     - $path"
        done
        print_msg $BLUE "当前目录内容:"
        ls -la "$WORKSPACE_DIR/$PACKAGE_NAME/" 2>/dev/null || echo "     目录不存在"
        return 1
    fi
    print_msg $GREEN "✅ 找到源文件: $PYTHON_FILE"
    return 0
}

# 颜色定义
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
PURPLE='\033[0;35m'
CYAN='\033[0;36m'
NC='\033[0m' # No Color

# 打印带颜色的消息
print_msg() {
    local color=$1
    local message=$2
    echo -e "${color}${message}${NC}"
}

# 检查ROS2环境
check_ros2_env() {
    print_msg $BLUE "🔧 检查ROS2环境..."
    source /opt/ros/foxy/setup.bash
    
    if [ -z "$ROS_DISTRO" ]; then
        print_msg $RED "❌ 错误: ROS2环境未正确配置!"
        exit 1
    fi
    
    print_msg $GREEN "✅ ROS2 $ROS_DISTRO 环境已加载"
}

# 停止现有节点
stop_existing_nodes() {
    print_msg $YELLOW "🛑 检查并停止现有节点..."
    
    # 停止所有相关进程
    pkill -f "$NODE_NAME" 2>/dev/null || true
    pkill -f "python.*$NODE_NAME" 2>/dev/null || true
    sleep 2
    
    # 检查是否还有运行的节点
    if ros2 node list 2>/dev/null | grep -q "$NODE_NAME"; then
        print_msg $YELLOW "⚠️  发现ROS2节点仍在运行，尝试强制停止..."
        pkill -9 -f "$NODE_NAME" 2>/dev/null || true
        sleep 2
    fi
    
    print_msg $GREEN "✅ 节点清理完成"
}

# 创建包结构
create_package_structure() {
    print_msg $BLUE "📁 创建包目录结构..."
    
    # 验证源文件
    if ! validate_source_file; then
        return 1
    fi
    
    cd "$WORKSPACE_DIR"
    mkdir -p "$PACKAGE_NAME/$PACKAGE_NAME"
    mkdir -p "$PACKAGE_NAME/launch"
    mkdir -p "$PACKAGE_NAME/resource"
    
    # 复制Python文件
    cp "$PYTHON_FILE" "$PACKAGE_NAME/$PACKAGE_NAME/"
    print_msg $GREEN "✅ 已复制Python文件: $PYTHON_FILE -> $PACKAGE_NAME/$PACKAGE_NAME/"
    
    # 创建__init__.py
    touch "$PACKAGE_NAME/$PACKAGE_NAME/__init__.py"
    
    # 创建package.xml
    cat > "$PACKAGE_NAME/package.xml" << EOF
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>$PACKAGE_NAME</name>
  <version>0.0.0</version>
  <description>Floor parsing ROS2 node</description>
  <maintainer email="ayler@example.com">ayler</maintainer>
  <license>Apache-2.0</license>
  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>
  <export>
    <build_type>ament_python</build_type>
  </export>
</package>
EOF
    
    # 创建setup.py
    cat > "$PACKAGE_NAME/setup.py" << EOF
from setuptools import setup
import os
from glob import glob

package_name = '$PACKAGE_NAME'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob('launch/*.py')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='ayler',
    maintainer_email='ayler@example.com',
    description='Floor parsing ROS2 node',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            '$NODE_NAME = $PACKAGE_NAME.$NODE_NAME:main',
        ],
    },
)
EOF
    
    # 创建launch文件
    cat > "$PACKAGE_NAME/launch/$NODE_NAME.launch.py" << EOF
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='$PACKAGE_NAME',
            executable='$NODE_NAME',
            name='$NODE_NAME',
            output='screen',
        ),
    ])
EOF
    
    # 创建resource文件
    touch "$PACKAGE_NAME/resource/$PACKAGE_NAME"
    
    print_msg $GREEN "✅ 包结构创建完成"
}

# 构建包
build_package() {
    print_msg $BLUE "🔨 构建ROS2包..."
    
    cd "$WORKSPACE_DIR"
    colcon build --packages-select $PACKAGE_NAME
    
    if [ $? -eq 0 ]; then
        print_msg $GREEN "✅ 构建成功!"
        return 0
    else
        print_msg $RED "❌ 构建失败!"
        return 1
    fi
}

# 加载环境
load_environment() {
    print_msg $BLUE "🌍 加载工作空间环境..."
    cd "$WORKSPACE_DIR"
    source install/setup.bash
    print_msg $GREEN "✅ 环境已加载"
}

# 显示配置信息
show_configuration() {
    print_msg $CYAN "🔧 当前配置信息"
    echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"
    print_msg $BLUE "📦 包名称: $PACKAGE_NAME"
    print_msg $BLUE "📁 工作空间: $WORKSPACE_DIR"
    print_msg $BLUE "🐍 源文件: $PYTHON_FILE"
    print_msg $BLUE "📝 日志文件: $LOG_FILE"
    
    # 验证源文件状态
    if validate_source_file; then
        print_msg $GREEN "✅ 源文件状态: 正常"
    else
        print_msg $RED "❌ 源文件状态: 异常"
    fi
    
    echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"
}

# 检查状态
check_status() {
    print_msg $CYAN "🔍 系统状态检查"
    echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"
    
    # 检查ROS2节点
    print_msg $BLUE "📊 ROS2节点列表:"
    if ros2 node list 2>/dev/null; then
        echo ""
    else
        print_msg $YELLOW "⚠️  无法获取节点列表或无运行节点"
    fi
    
    # 检查topics
    print_msg $BLUE "📡 可用Topics:"
    ros2 topic list 2>/dev/null || print_msg $YELLOW "⚠️  无法获取topic列表"
    
    # 检查进程状态
    print_msg $BLUE "🔄 Floor Publisher进程状态:"
    if pgrep -f "$NODE_NAME" > /dev/null; then
        print_msg $GREEN "✅ $NODE_NAME进程正在运行"
        print_msg $CYAN "PID: $(pgrep -f $NODE_NAME)"
    else
        print_msg $YELLOW "⚠️  $NODE_NAME进程未运行"
    fi
    
    # 检查日志
    if [ -f "$LOG_FILE" ]; then
        print_msg $BLUE "📝 最新日志 (最后10行):"
        tail -10 "$LOG_FILE"
    else
        print_msg $YELLOW "⚠️  日志文件不存在: $LOG_FILE"
    fi
    
    echo "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"
}

# 启动节点（前台）
start_node_foreground() {
    print_msg $GREEN "🚀 启动节点（前台模式）"
    print_msg $CYAN "使用 Ctrl+C 停止节点"
    echo ""
    
    # 使用正确的Python版本
    PYTHON_CMD="$PYTHON_CMD_DIR"
    if [ ! -f "$PYTHON_CMD" ]; then
        PYTHON_CMD="python3"
    fi
    
    print_msg $BLUE "🐍 使用Python: $PYTHON_CMD"
    print_msg $BLUE "📝 Python版本: $($PYTHON_CMD --version)"
    
    # 启动节点
    $PYTHON_CMD "$PYTHON_FILE"
}

# 启动节点（后台）
start_node_background() {
    print_msg $GREEN "🚀 启动节点（后台模式）"
    
    # 使用正确的Python版本
    PYTHON_CMD="/bin/python3"
    if [ ! -f "$PYTHON_CMD" ]; then
        PYTHON_CMD="python3"
    fi
    
    print_msg $BLUE "🐍 使用Python: $PYTHON_CMD"
    
    # 后台启动节点
    nohup $PYTHON_CMD "$PYTHON_FILE" > "$LOG_FILE" 2>&1 &
    NODE_PID=$!
    
    sleep 2
    
    # 检查节点是否启动成功
    if ps -p $NODE_PID > /dev/null; then
        print_msg $GREEN "✅ 节点已成功启动!"
        print_msg $CYAN "📊 节点PID: $NODE_PID"
        print_msg $CYAN "📝 日志文件: $LOG_FILE"
        
        # 验证ROS2节点注册
        sleep 3
        if ros2 node list 2>/dev/null | grep -q "$NODE_NAME"; then
            print_msg $GREEN "✅ ROS2节点已注册"
        else
            print_msg $YELLOW "⚠️  ROS2节点未检测到，但进程正在运行"
        fi
        
        print_msg $BLUE "📋 管理命令:"
        echo "  查看日志: tail -f $LOG_FILE"
        echo "  查看topics: ros2 topic list"
        echo "  查看节点: ros2 node list"
        echo "  停止节点: kill $NODE_PID"
        echo "  或者: pkill -f $NODE_NAME"
    else
        print_msg $RED "❌ 节点启动失败!"
        print_msg $YELLOW "📝 检查日志: cat $LOG_FILE"
        return 1
    fi
}

# 主菜单
show_menu() {
    clear
    print_msg $PURPLE "╔══════════════════════════════════════════════════════════════╗"
    print_msg $PURPLE "║            ROS2 Floor Parse Package - 统一管理工具            ║"
    print_msg $PURPLE "╚══════════════════════════════════════════════════════════════╝"
    echo ""
    print_msg $CYAN "请选择操作:"
    echo ""
    print_msg $GREEN "  [1] 🏗️  完整构建 - 创建包结构 + 构建 + 验证"
    print_msg $GREEN "  [2] 🔨 仅构建 - 只构建包，不启动"
    print_msg $GREEN "  [3] 🚀 构建并后台启动 - 构建 + 后台运行节点"
    print_msg $GREEN "  [4] 🖥️  构建并前台启动 - 构建 + 前台运行节点（可看输出）"
    print_msg $YELLOW "  [5] ▶️  仅启动节点（前台） - 假设已构建"
    print_msg $YELLOW "  [6] 🌙 仅启动节点（后台） - 假设已构建"
    print_msg $BLUE "  [7] 📊 检查状态 - 查看节点和系统状态"
    print_msg $BLUE "  [8] 📝 查看配置信息"
    print_msg $BLUE "  [9] 🛑 停止所有节点"
    print_msg $BLUE "  [10] 📝 查看实时日志"
    print_msg $RED "  [0] 🚪 退出"
    echo ""
    print_msg $CYAN "━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━"
}

# 主逻辑
main() {
    # 初始化环境
    check_ros2_env
    
    # 如果有命令行参数，直接执行对应功能
    if [ $# -gt 0 ]; then
        case $1 in
            "build")
                stop_existing_nodes
                create_package_structure
                build_package
                load_environment
                ;;
            "start-bg")
                stop_existing_nodes
                create_package_structure
                build_package
                load_environment
                start_node_background
                ;;
            "start-fg")
                stop_existing_nodes
                create_package_structure
                build_package
                load_environment
                start_node_foreground
                ;;
            "status")
                check_status
                ;;
            "stop")
                stop_existing_nodes
                ;;
            *)
                echo "用法: $0 [build|start-bg|start-fg|status|stop]"
                exit 1
                ;;
        esac
        return
    fi
    
    # 交互式菜单
    while true; do
        show_menu
        read -p "请输入选择 [0-9]: " choice
        
        case $choice in
            1)
                print_msg $BLUE "执行完整构建流程..."
                stop_existing_nodes
                create_package_structure
                if build_package; then
                    load_environment
                    print_msg $GREEN "🎉 完整构建完成!"
                    check_status
                fi
                read -p "按Enter键继续..."
                ;;
            2)
                print_msg $BLUE "执行仅构建..."
                stop_existing_nodes
                create_package_structure
                build_package
                read -p "按Enter键继续..."
                ;;
            3)
                print_msg $BLUE "执行构建并后台启动..."
                stop_existing_nodes
                create_package_structure
                if build_package; then
                    load_environment
                    start_node_background
                fi
                read -p "按Enter键继续..."
                ;;
            4)
                print_msg $BLUE "执行构建并前台启动..."
                stop_existing_nodes
                create_package_structure
                if build_package; then
                    load_environment
                    start_node_foreground
                fi
                read -p "按Enter键继续..."
                ;;
            5)
                print_msg $BLUE "仅启动节点（前台）..."
                load_environment
                start_node_foreground
                read -p "按Enter键继续..."
                ;;
            6)
                print_msg $BLUE "仅启动节点（后台）..."
                load_environment
                start_node_background
                read -p "按Enter键继续..."
                ;;
            7)
                check_status
                read -p "按Enter键继续..."
                ;;
            8)
                show_configuration
                read -p "按Enter键继续..."
                ;;
            9)
                stop_existing_nodes
                read -p "按Enter键继续..."
                ;;
            10)
                if [ -f "$LOG_FILE" ]; then
                    print_msg $BLUE "📝 实时日志 (按Ctrl+C退出):"
                    tail -f "$LOG_FILE"
                else
                    print_msg $YELLOW "⚠️  日志文件不存在: $LOG_FILE"
                    read -p "按Enter键继续..."
                fi
                ;;
            0)
                print_msg $GREEN "👋 再见!"
                exit 0
                ;;
            *)
                print_msg $RED "❌ 无效选择，请重新输入"
                sleep 1
                ;;
        esac
    done
}

# 设置清理函数
cleanup() {
    print_msg $YELLOW "🧹 脚本退出，清理环境..."
}

trap cleanup EXIT

# 启动主程序
main "$@"